#include "stdio.h"
#include "math.h"
#include "stdlib.h"
#include "mechine_offset_adj.h"

#define NEGATIVE  0
#define POSITIVE  1

//#define LOG printf
#define LOG(...)

int adj_init(adj_t *adj, int offset)
{
  if(adj == NULL)
    return -1;
  adj->dir = NEGATIVE;
  adj->offset = offset;
  adj->real=0;
  adj->dest=0;
  adj->last= 0;
  adj->enable = 1;
  return 0;
}

int adj_disable(adj_t *adj)
{
  if(adj == NULL)
    return -1;
  adj->enable = 0;
  return 0;
}

int adj_encode_value(adj_t *adj, int encode_val)
{
  int abs_off;
  
  if(adj == NULL || adj->enable != 1)
    return encode_val;
  
  adj->encode =  encode_val;
  LOG("encode val:%d\n", encode_val);
  abs_off = abs(encode_val - adj->last);

  if(adj->dir == NEGATIVE)
  {
    if(encode_val <= adj->last)
    {      
      adj->last=encode_val;
      adj->real=encode_val;
    }
    else
    {
      if(abs_off < adj->offset)
        adj->real = adj->last;
      else
      {
        adj->dir=POSITIVE;
        adj->last = encode_val;
        adj->real = encode_val - adj->offset;
      }
    }
  }
  else
  {
    if(encode_val >= adj->last)
    {   
      adj->last=encode_val;
      adj->real=encode_val - adj->offset;
    }
    else
    {
      if(abs_off < adj->offset)
        adj->real = adj->last - adj->offset;
      else
      {
        adj->dir=NEGATIVE;
        adj->last = encode_val;
        adj->real = encode_val;
      }
    }
  }
  LOG("real:%d\n", adj->real);
  return adj->real;
}

int adj_ctrl_param(adj_t *adj, int encode_val, int set_val)
{
  int real_val=0;
  
  if(adj == NULL || adj->enable != 1)
    return set_val;
  
  real_val = adj_encode_value(adj, encode_val);
  if(set_val > real_val)//POSITIVE
  {
    adj->dest = set_val + adj->offset; 
  }
  else if(set_val < real_val)//NEGATIVE
  {
    adj->dest = set_val; 
  }
  else
  {
    LOG("idle\n");
  }
  
  LOG("%d\n", adj->dest);  
  return adj->dest;
}


/*
* test
* gcc
*/
/*
void main(void)
{
  int setv;
  int encodev;
  adj_t adj;
  
  adj_init(&adj);
  
  while(1)
    
  {
    
    LOG("\n****************\n");
    
    LOG("adj ctrl input:c \n");
    
    if(getchar()=='c')
      
    {
      
      LOG("\nset destance:");
      
      scanf("%d",&setv);
      
      LOG("\n");
      
      LOG("\nset encodev:");
      
      scanf("%d",&encodev);
      
      LOG("\n");
      
      adj_ctrl_param(&adj, encodev, setv);
      
    }
    
    else
      
    {
      
      LOG("\nset encodev:");
      
      scanf("%d",&encodev);
      
      LOG("\n");
      
      adj_encode_value(&adj,encodev);
      
    }
    
    
    
    LOG("dir:%s\n",adj.dir==0?"NEGATIVE":"POSITIVE");
    
    LOG("offset:%d\n",adj.offset);
    
    LOG("dest:%d\n",adj.dest);
    
    LOG("last:%d\n",adj.last);
    
    LOG("real:%d\n",adj.real);
    
    getchar();
    
  }
  
}
*/